#include "MeasurE616RectExtract.h"

MeasurE616RectExtract::MeasurE616RectExtract()
{

}

//射线旋转边缘提取，用于L和D圆,输出为两角度内的边缘
int MeasurE616RectExtract::KirschEdgeCircle(cv::Mat srcimg,int threshold, int startangle, int endangle, vector<cv::Point>& contours){
    if(srcimg.empty())
        return -1;
    if(srcimg.channels()==3)
        cv::cvtColor(srcimg,srcimg,CV_BGR2GRAY);
    cv::Mat edge = krisch(srcimg);
    int radius=5;
    float percent=0.8;
    cv::threshold(edge, edge, threshold, 255, cv::THRESH_BINARY);
    cv::Point center(edge.cols/2,edge.rows/2);
    FindEdge(edge, center, contours, radius,percent,0);
    contours=ContoursCut(contours, startangle, endangle);
    return 0;
}

//小圆边缘提取
int MeasurE616RectExtract::KirschEdgeSmallCircle(cv::Mat srcimg,int threshold,int num, vector<cv::Point>& contours){
    if(srcimg.empty())
        return -1;
    if(srcimg.channels()==3)
        cv::cvtColor(srcimg,srcimg,CV_BGR2GRAY);
    cv::Mat edge = krisch(srcimg);
    cv::threshold(edge, edge, threshold, 255, cv::THRESH_BINARY);
    //cv::imshow("edge",edge);
//    cv::imwrite("edgethresh.jpg",edge);
    cv::Point center(edge.cols/2,edge.rows/2);
    edge.at<uchar>(edge.rows/2,edge.cols/2)=0;
    if(num==1){
        FindEdge(edge, center, contours, 5,0.8,40);
        contours=ContoursCut(contours, 0, 360);
    }
    else if(num==2){
        FindEdge(edge, center, contours, 5,0.8,40);
        contours=ContoursCut(contours, 0, 360);
    }
    else if(num==3){
        FindEdge(edge, center, contours, 5,0.8,48);
        contours=ContoursCut(contours, 0, 360);
    }
    else if(num==4){
        FindEdge(edge, center, contours, 5,0.8,40);
        contours=ContoursCut(contours, 0, 360);
    }
    else if(num==5){
        FindEdge(edge, center, contours, 5,0.8,52);
        contours=ContoursCut(contours, 0, 360);
    }
    else if(num==6){
        FindEdge(edge, center, contours, 5,0.8,50);
        contours=ContoursCut(contours, 0, 360);
    }
    else if(num==7){
        FindEdge(edge, center, contours, 5,0.8,48);
        contours=ContoursCut(contours, 0, 360);
    }
    return 0;
}

int MeasurE616RectExtract::GetSmallCircle(cv::Mat srcimg,int threshold, int num, vector<float>& result){
    if(srcimg.empty())
        return -1;
    if(srcimg.channels()==3)
        cv::cvtColor(srcimg,srcimg,CV_BGR2GRAY);
    vector<cv::Point> contours;
    KirschEdgeSmallCircle(srcimg,threshold,num,contours);
    result=FitCircle(contours);
    return 0;
}

//线扫边缘提取，用于R圆，输出为30个点(15+15)的边缘
int MeasurE616RectExtract::KirschEdgeLine(cv::Mat srcimg,int threshold,int orientation, vector<cv::Point>& contours){
    if(srcimg.empty())
        return -1;
    if(srcimg.channels()==3)
        cv::cvtColor(srcimg,srcimg,CV_BGR2GRAY);
    int radius=5;
    float percent=0.8;
    cv::Mat edge = krisch(srcimg);
    int startline;
    cv::threshold(edge, edge, threshold, 255, cv::THRESH_BINARY);
    if(orientation==1){
        startline=srcimg.rows/2;
        for(int i=radius+1;i<edge.cols-radius-1;++i){
            cv::Point seed(i,startline);
            cv::Point contourpoint(i,seed.y-1);
            while(!PointDense(edge,contourpoint,radius,percent)||abs(edge.at<uchar>(contourpoint.y,contourpoint.x)-edge.at<uchar>(seed.y,seed.x))<15){
                contourpoint.y-=1;
                if(contourpoint.y==0)
                    break;
            }
            contours.push_back(contourpoint);
        }
        contours=GetSegments(contours,vector<int>{45,60,235,250});
    }
    else if(orientation==2){
        startline=srcimg.cols/2;
        for(int i=radius+1;i<edge.rows-radius-1;++i){
            cv::Point seed(startline,i);
            cv::Point contourpoint(seed.x+1,i);
            while(!PointDense(edge,contourpoint,radius,percent)||abs(edge.at<uchar>(contourpoint.y,contourpoint.x)-edge.at<uchar>(seed.y,seed.x))<15){
                contourpoint.x+=1;
                if(contourpoint.x>=edge.cols)
                    break;
            }
            contours.push_back(contourpoint);
        }
        contours=GetSegments(contours,vector<int>{50,65,240,255});
    }
    else if(orientation==3){
        startline=srcimg.rows/2;
        for(int i=radius+1;i<edge.cols-radius-1;++i){
            cv::Point seed(i,startline);
            cv::Point contourpoint(i,seed.y+1);
            while(!PointDense(edge,contourpoint,radius,percent)||abs(edge.at<uchar>(contourpoint.y,contourpoint.x)-edge.at<uchar>(seed.y,seed.x))<15){
                contourpoint.y+=1;
                if(contourpoint.y>=edge.rows)
                    break;
            }
            contours.push_back(contourpoint);
        }
        contours=GetSegments(contours,vector<int>{65,80,250,265});
    }
    else if(orientation==4){
        startline=srcimg.cols/2;
        for(int i=radius+1;i<edge.rows-radius-1;++i){
            cv::Point seed(startline,i);
            cv::Point contourpoint(seed.x-1,i);
            while(!PointDense(edge,contourpoint,radius,percent)||abs(edge.at<uchar>(contourpoint.y,contourpoint.x)-edge.at<uchar>(seed.y,seed.x))<15){
                contourpoint.x-=1;
                if(contourpoint.x==0)
                    break;
            }
            contours.push_back(contourpoint);
        }
        contours=GetSegments(contours,vector<int>{65,80,250,265});
    }
    return 0;
}

int MeasurE616RectExtract::GetDatum(cv::Mat srcimg,vector<cv::Point2f>& datum){
    if(srcimg.empty())
        return -1;
    if(srcimg.channels()==3)
        cv::cvtColor(srcimg,srcimg,CV_BGR2GRAY);
    cv::Mat toplineimg=srcimg(cv::Rect(cv::Point(1806,56),cv::Point(2702,309)));
    cv::Mat topedge = krisch(toplineimg);
    cv::threshold(topedge, topedge, 150, 255, cv::THRESH_BINARY);
    //cv::imshow("top",topedge);
    vector<cv::Point> toplinecontour;
    ScanLine(topedge,3,toplinecontour,0,5,0.8);
    for(int i=0;i<toplinecontour.size();++i){
        toplinecontour[i].x+=1806;
        toplinecontour[i].y+=56;
    }
    vector<float> toplinepara;
    LineFitLeastSquares(toplinecontour,toplinepara);
    cv::Point2f topline;
    topline.x=(2702+1806)/2;
    topline.y=toplinepara[0]*topline.x+toplinepara[1];
    //cout<<topline<<endl;
    cv::Mat leftlineimg=srcimg(cv::Rect(cv::Point(32,1120),cv::Point(192,1760)));
    cv::Mat leftedge = krisch(leftlineimg);
    cv::threshold(leftedge, leftedge, 150, 255, cv::THRESH_BINARY);
    //cv::imshow("left",leftedge);
    vector<cv::Point> leftlinecontour;
    ScanLine(leftedge,2,leftlinecontour,0,3,0.8);
    for(int i=0;i<leftlinecontour.size();++i){
        leftlinecontour[i].x+=32;
        leftlinecontour[i].y+=1120;
    }
    for(int i=0;i<leftlinecontour.size();++i){
        cv::Point temp=leftlinecontour[i];
        leftlinecontour[i].x=temp.y;
        leftlinecontour[i].y=temp.x;
    }
    vector<float> leftlinepara;
    LineFitLeastSquares(leftlinecontour,leftlinepara);
    cv::Point2f leftline;
    leftline.y=(1120+1760)/2;
    leftline.x=leftlinepara[0]*leftline.x+leftlinepara[1];
    //cout<<leftline<<endl;
    cv::Point2f crosspoint;
    crosspoint.x=(-leftlinepara[1]/leftlinepara[0]-toplinepara[1])/(toplinepara[0]-1/leftlinepara[0]);
    crosspoint.y=crosspoint.x*toplinepara[0]+toplinepara[1];
    //cout<<crosspoint<<endl;
    datum.clear();
    datum.push_back(topline);
    datum.push_back(leftline);
    datum.push_back(crosspoint);
    return 0;
}

float MeasurE616RectExtract::Point2Line(cv::Point Point, vector<cv::Point2f> datum, int flag){
    float A,B,C;
    if(flag==1){
        A=datum[0].y-datum[2].y;
        B=datum[2].x-datum[0].x;
        C=(datum[0].x-datum[2].x)*datum[2].y-datum[2].x*(datum[0].y-datum[2].y);
    }
    else if(flag==0){
        A=datum[1].y-datum[2].y;
        B=datum[2].x-datum[1].x;
        C=(datum[1].x-datum[2].x)*datum[2].y-datum[2].x*(datum[1].y-datum[2].y);
    }
    else{
        return -1;
    }
    if(A*A+B*B==0){
        return -1;
    }
    float dist;
    dist=fabs((A*Point.x+B*Point.y+C)/sqrt(A*A+B*B));
    return dist;
}

int MeasurE616RectExtract::ScanLine(cv::Mat srcimg, int lineori, vector<cv::Point>& contours,int startline, int radius, float percent){
    if(srcimg.empty()||srcimg.cols<radius*2+2||srcimg.rows<radius*2+2)
        return -1;
    if(srcimg.channels()==3)
        cv::cvtColor(srcimg,srcimg,CV_BGR2GRAY);
    if(lineori==1){
        for(int i=radius+1;i<srcimg.cols-radius-1;++i){
            cv::Point seed(i,startline);
            cv::Point contourpoint(i,seed.y-1);
            while(!PointDense(srcimg,contourpoint,radius,percent)||abs(srcimg.at<uchar>(contourpoint.y,contourpoint.x)-srcimg.at<uchar>(seed.y,seed.x))<15){
                contourpoint.y-=1;
                if(contourpoint.y==0)
                    break;
            }
            contours.push_back(contourpoint);
        }
    }
    else if(lineori==2){
        for(int i=radius+1;i<srcimg.rows-radius-1;++i){
            cv::Point seed(startline,i);
            cv::Point contourpoint(seed.x+1,i);
            while(!PointDense(srcimg,contourpoint,radius,percent)||abs(srcimg.at<uchar>(contourpoint.y,contourpoint.x)-srcimg.at<uchar>(seed.y,seed.x))<15){
                contourpoint.x+=1;
                if(contourpoint.x>=srcimg.cols)
                    break;
            }
            contours.push_back(contourpoint);
        }
    }
    else if(lineori==3){
        for(int i=radius+1;i<srcimg.cols-radius-1;++i){
            cv::Point seed(i,startline);
            cv::Point contourpoint(i,seed.y+1);
            while(!PointDense(srcimg,contourpoint,radius,percent)||abs(srcimg.at<uchar>(contourpoint.y,contourpoint.x)-srcimg.at<uchar>(seed.y,seed.x))<15){
                contourpoint.y+=1;
                if(contourpoint.y>=srcimg.rows)
                    break;
            }
            contours.push_back(contourpoint);
        }
    }
    else if(lineori==4){
        for(int i=radius+1;i<srcimg.rows-radius-1;++i){
            cv::Point seed(startline,i);
            cv::Point contourpoint(seed.x-1,i);
            while(!PointDense(srcimg,contourpoint,radius,percent)||abs(srcimg.at<uchar>(contourpoint.y,contourpoint.x)-srcimg.at<uchar>(seed.y,seed.x))<15){
                contourpoint.x-=1;
                if(contourpoint.x==0)
                    break;
            }
            contours.push_back(contourpoint);
        }
    }
    return 0;
}

int MeasurE616RectExtract::GetRectEdge(cv::Mat srcimg,int threshold,int num, vector<float>& dists){
    dists.clear();
    vector<cv::Point> contours;
    vector<vector<cv::Point>> edgecontours;
    if(num==1){
        vector<int> segments{40,140,160,200,220,320,340,20};
        vector<int> distseg{220,810,180,500};
        KirschEdge(srcimg,threshold,contours,edgecontours, segments);
        FitLineAndGetDist(edgecontours,distseg,dists);
    }
    else if(num==2){
        vector<int> segments{45,135,155,180,225,315,345,25};
        vector<int> distseg{120,416,96,270};
        KirschEdge(srcimg,threshold,contours,edgecontours, segments);
        vector<vector<cv::Point>> topbottomcontours;
        topbottomcontours.push_back(edgecontours[0]);
        topbottomcontours.push_back(edgecontours[2]);
        float ypos=GetMidLineY(topbottomcontours, srcimg.cols/2);
        FitLineAndGetDist(edgecontours,distseg,dists);
        float xpos=GetLineX(edgecontours[1],srcimg.rows/2);
        dists.push_back(ypos);
        dists.push_back(xpos);
    }
    else if(num==3){
        cv::Mat edge = krisch(srcimg.clone());
        cv::threshold(edge, edge, threshold, 255, cv::THRESH_BINARY);
        int startline=srcimg.cols/2;
        for(int i=5+1;i<edge.rows-5-1;++i){
            cv::Point seed(startline,i);
            cv::Point contourpoint(seed.x-1,i);
            while(!PointDense(edge,contourpoint,5,0.8)||abs(edge.at<uchar>(contourpoint.y,contourpoint.x)-edge.at<uchar>(seed.y,seed.x))<15){
                contourpoint.x-=1;
                if(contourpoint.x==0)
                    break;
            }
            contours.push_back(contourpoint);
        }
        contours=GetSegments(contours,vector<int>{80,180});
        edgecontours.push_back(contours);
        for(int i=0;i<contours.size();++i){
            cv::circle(srcimg,contours[i],1,255,-1);
        }
        ScanRightLine(srcimg,contours);
        edgecontours.push_back(contours);
        vector<int> distseg{98,238};
        FitLineAndGetDist(edgecontours,distseg,dists);
    }
    else if(num==4){
        vector<int> segments{262,274,86,98};
        vector<int> distseg{50,90,50,90};
        KirschEdge(srcimg,threshold,contours,edgecontours, segments);
        FitLineAndGetDist(edgecontours,distseg,dists);
    }
    else if(num==5){
        vector<int> segments{266,278,82,94};
        vector<int> distseg{70,110,70,110};
        KirschEdge(srcimg,threshold,contours,edgecontours, segments);
        FitLineAndGetDist(edgecontours,distseg,dists);
    }
}

//边缘提取，截取片段，生成每一段的边缘点
int MeasurE616RectExtract::KirschEdge(cv::Mat srcimg,int threshold, vector<cv::Point>& contours, vector<vector<cv::Point>>& linecontours, vector<int> segment){
    if(srcimg.empty())
        return -1;
    if(srcimg.channels()==3)
        cv::cvtColor(srcimg,srcimg,CV_BGR2GRAY);
    int radius=5;
    float percent=0.8;
    int radthresh=0;
    cv::Mat point=srcimg.clone();
    cv::Mat edge = krisch(srcimg);
    cv::threshold(edge, edge, threshold, 255, cv::THRESH_BINARY);
    cv::Point center(edge.cols/2,edge.rows/2);
    edge.at<uchar>(edge.rows/2,edge.cols/2)=0;
    FindEdge(edge, center, contours, radius,percent,radthresh);
    linecontours.clear();
    for(int i=0;i<segment.size()/2;++i){
        vector<cv::Point> segmentedge=ContoursCut(contours, segment[i*2+0], segment[i*2+1]);
        linecontours.push_back(segmentedge);
    }
    return 0;
}

//线扫边缘提取的截取函数，输入为截取位置的vector
vector<cv::Point> MeasurE616RectExtract::GetSegments(vector<cv::Point> contours, vector<int> segments){
    int segnum=segments.size()/2;
    vector<cv::Point> segcontour;
    for(int i=0;i<segnum;++i){
        for(int j=segments[i*2];j<segments[i*2+1];++j){
            segcontour.push_back(contours[j]);
        }
    }
    return segcontour;
}

/* Krisch 边缘检测算法*/
cv::Mat MeasurE616RectExtract::krisch(cv::InputArray src,int borderType)
{
    //存储八个卷积结果
    vector<cv::Mat> eightEdge;
    eightEdge.clear();
    /*第1步：图像矩阵与8 个 卷积核卷积*/
    /*Krisch 的 8 个卷积核均不是可分离的*/
    //图像矩阵与 k1 卷积
    cv::Mat k1 = (cv::Mat_<float>(3, 3) << 5, 5, 5, -3, 0, -3, -3, -3, -3);
    cv::Mat src_k1;
    conv2D(src, k1, src_k1, CV_32FC1);
    cv::convertScaleAbs(src_k1, src_k1);
    eightEdge.push_back(src_k1);
    //图像矩阵与 k2 卷积
    cv::Mat k2 = (cv::Mat_<float>(3, 3) << -3, -3, -3, -3, 0, -3, 5, 5, 5);
    cv::Mat src_k2;
    conv2D(src, k2, src_k2, CV_32FC1);
    cv::convertScaleAbs(src_k2, src_k2);
    eightEdge.push_back(src_k2);
    //图像矩阵与 k3 卷积
    cv::Mat k3 = (cv::Mat_<float>(3, 3) << -3, 5, 5, -3, 0, 5, -3, -3, -3);
    cv::Mat src_k3;
    conv2D(src, k3, src_k3, CV_32FC1);
    cv::convertScaleAbs(src_k3, src_k3);
    eightEdge.push_back(src_k3);
    //图像矩阵与 k4 卷积
    cv::Mat k4 = (cv::Mat_<float>(3, 3) << -3, -3, -3, 5, 0, -3, 5, 5, -3);
    cv::Mat src_k4;
    conv2D(src, k4, src_k4, CV_32FC1);
    cv::convertScaleAbs(src_k4, src_k4);
    eightEdge.push_back(src_k4);
    //图像矩阵与 k5 卷积
    cv::Mat k5 = (cv::Mat_<float>(3, 3) << -3, -3, 5, -3, 0, 5, -3, -3, 5);
    cv::Mat src_k5;
    conv2D(src, k5, src_k5, CV_32FC1);
    cv::convertScaleAbs(src_k5, src_k5);
    eightEdge.push_back(src_k5);
    //图像矩阵与 k6 卷积
    cv::Mat k6 = (cv::Mat_<float>(3, 3) << 5, -3, -3, 5, 0, -3, 5, -3, -3);
    cv::Mat src_k6;
    conv2D(src, k6, src_k6, CV_32FC1);
    cv::convertScaleAbs(src_k6, src_k6);
    eightEdge.push_back(src_k6);
    //图像矩阵与 k7 卷积
    cv::Mat k7 = (cv::Mat_<float>(3, 3) << -3, -3, -3, -3, 0, 5, -3, 5, 5);
    cv::Mat src_k7;
    conv2D(src, k7, src_k7, CV_32FC1);
    cv::convertScaleAbs(src_k7, src_k7);
    eightEdge.push_back(src_k7);
    //图像矩阵与 k8 卷积
    cv::Mat k8 = (cv::Mat_<float>(3, 3) << 5, 5, -3, 5, 0, -3, -3, -3, -3);
    cv::Mat src_k8;
    conv2D(src, k8, src_k8, CV_32FC1);
    cv::convertScaleAbs(src_k8, src_k8);
    eightEdge.push_back(src_k8);
    /*第二步：将求得的八个卷积结果,取对应位置的最大值，作为最后的边缘输出*/
    cv::Mat krischEdge = eightEdge[0].clone();
    for (int i = 0; i < 8; i++)
    {
        cv::max(krischEdge, eightEdge[i], krischEdge);
    }
    return krischEdge;
}

//获取需要测量的距离
void MeasurE616RectExtract::FitLineAndGetDist(vector<vector<cv::Point>> linecontours,vector<int> coordinates, vector<float>& dist){
    dist.clear();
    if(linecontours.size()==4){
        vector<float> toplinepara,leftlinepara,bottomlinepara,rightlinepara;
        LineFitLeastSquares(linecontours[0],toplinepara);
        for(int i=0;i<linecontours[1].size();++i){
            cv::Point temp=linecontours[1][i];
            linecontours[1][i].x=temp.y;
            linecontours[1][i].y=temp.x;
        }
        LineFitLeastSquares(linecontours[1],leftlinepara);
        LineFitLeastSquares(linecontours[2],bottomlinepara);
        for(int i=0;i<linecontours[3].size();++i){
            cv::Point temp=linecontours[3][i];
            linecontours[3][i].x=temp.y;
            linecontours[3][i].y=temp.x;
        }
        LineFitLeastSquares(linecontours[3],rightlinepara);
        float sum=0;
        for(int i=coordinates[0];i<coordinates[1];++i){
            sum+=fabs((i*toplinepara[0]+toplinepara[1])-(i*bottomlinepara[0]+bottomlinepara[1]));
        }
        dist.push_back(sum/(coordinates[1]-coordinates[0]));
        sum=0;
        for(int i=coordinates[2];i<coordinates[3];++i){
            sum+=fabs((i*leftlinepara[0]+leftlinepara[1])-(i*rightlinepara[0]+rightlinepara[1]));
        }
        dist.push_back(sum/(coordinates[3]-coordinates[2]));
    }
    if(linecontours.size()==2&&coordinates.size()==2){
        vector<float> leftlinepara,rightlinepara;
        for(int i=0;i<linecontours[0].size();++i){
            cv::Point temp=linecontours[0][i];
            linecontours[0][i].x=temp.y;
            linecontours[0][i].y=temp.x;
        }
        LineFitLeastSquares(linecontours[0],leftlinepara);
        for(int i=0;i<linecontours[1].size();++i){
            cv::Point temp=linecontours[1][i];
            linecontours[1][i].x=temp.y;
            linecontours[1][i].y=temp.x;
        }
        LineFitLeastSquares(linecontours[1],rightlinepara);
        float sum=0;
        for(int i=coordinates[0];i<coordinates[1];++i){
            sum+=fabs((i*leftlinepara[0]+leftlinepara[1])-(i*rightlinepara[0]+rightlinepara[1]));
        }
        dist.push_back(sum/(coordinates[1]-coordinates[0]));
    }
    if(linecontours.size()==2&&coordinates.size()==4){
        vector<float> bottomlinepara;
        LineFitLeastSquares(linecontours[0],bottomlinepara);
        float sum=0;
        for(int i=coordinates[0];i<coordinates[1];++i){
            sum+=fabs(i*bottomlinepara[0]+bottomlinepara[1]);
        }
        dist.push_back(sum/(coordinates[1]-coordinates[0]));
        vector<float> toplinepara;
        LineFitLeastSquares(linecontours[1],toplinepara);
        sum=0;
        for(int i=coordinates[0];i<coordinates[1];++i){
            sum+=fabs(i*toplinepara[0]+toplinepara[1]);
        }
        dist.push_back(sum/(coordinates[1]-coordinates[0]));
    }
}

//拟合直线，返回直线参数k,b
void MeasurE616RectExtract::LineFitLeastSquares(vector<cv::Point> contours, vector<float> &vResult)
{
    float A = 0.0;
    float B = 0.0;
    float C = 0.0;
    float D = 0.0;
    float E = 0.0;
    float F = 0.0;

    for (int i = 0; i < contours.size(); i++)
    {
        A += contours[i].x * contours[i].x;
        B += contours[i].x;
        C += contours[i].x * contours[i].y;
        D += contours[i].y;
    }
    float a, b, temp = 0;
    if( temp = (contours.size()*A - B*B) )
    {
        a = (contours.size()*C - B*D) / temp;
        b = (A*D - B*C) / temp;
    }
    else
    {
        a = 1;
        b = 0;
    }
    float Xmean, Ymean;
    Xmean = B / contours.size();
    Ymean = D / contours.size();

    float tempSumXX = 0.0, tempSumYY = 0.0;
    for (int i=0; i<contours.size(); i++)
    {
        tempSumXX += (contours[i].x - Xmean) * (contours[i].x - Xmean);
        tempSumYY += (contours[i].y - Ymean) * (contours[i].y - Ymean);
        E += (contours[i].x - Xmean) * (contours[i].y - Ymean);
    }
    F = sqrt(tempSumXX) * sqrt(tempSumYY);

    float r;
    r = E / F;

    vResult.push_back(a);
    vResult.push_back(b);
    vResult.push_back(r*r);
}

void MeasurE616RectExtract::ScanRightLine(cv::Mat srcimg, vector<cv::Point>& contours){
    contours.clear();
    for(int i=98; i<242; i=i+2){
        for(int j=srcimg.cols-4;j>srcimg.cols-50;--j){
            cv::Mat roi=srcimg(cv::Rect(j-3,i-3,7,7));
            if(cv::mean(roi)[0]>60.0){
                continue;}
            else{
                if(PixelDense(roi,40)){
                    contours.push_back(cv::Point(j,i));
                    break;
                }
            }
        }
    }
}

bool MeasurE616RectExtract::PixelDense(cv::Mat srcimg, int thresh){
    int posnum=0;
    for(int i=0;i<srcimg.rows;++i){
        for(int j=0;j<srcimg.cols;++j){
            if(srcimg.at<uchar>(i,j)<thresh){
                posnum++;
            }
        }
    }
    if(posnum>=(srcimg.rows*srcimg.cols*0.45)&&posnum<=(srcimg.rows*srcimg.cols*0.6)){
        return true;
    }
    else{
        return false;
    }
}

/*离散的二维卷积运算*/
void MeasurE616RectExtract::conv2D(cv::InputArray _src, cv::InputArray _kernel, cv::OutputArray _dst, int ddepth, cv::Point anchor, int borderType)
{
    //卷积核顺时针旋转180
    cv::Mat kernelFlip;
    cv::flip(_kernel, kernelFlip, -1);
    //针对每一个像素,领域对应元素相乘然后相加
    cv::filter2D(_src, _dst, CV_32FC1, _kernel, anchor, 0.0, borderType);
}

//旋转射线求边缘，输入为图片，旋转中心点，密度判断的半径radius和比例percent
int MeasurE616RectExtract::FindEdge(cv::Mat srcimg, cv::Point seed, vector<cv::Point>& contours, int radius, float percent, int radthresh){
    if(srcimg.empty())
        return -1;
    if(srcimg.channels()==3)
        cv::cvtColor(srcimg,srcimg,CV_BGR2GRAY);
    contours.clear();
    cv::Point center=seed;
    float theta;
    for(int i=0;i<180;++i){
        theta=i;
        float k=tan(theta/180.0*PI);
        float b=center.y-k*center.x;
        if(theta>=0&&theta<=45){
            cv::Point contourPoint1(center.x+1+radthresh,(center.x+1+radthresh)*k+b);
            cv::Point contourPoint2(center.x-1-radthresh,(center.x-1-radthresh)*k+b);
            while(!PointDense(srcimg,contourPoint1,radius,percent)||abs(srcimg.at<uchar>(contourPoint1.y,contourPoint1.x)-srcimg.at<uchar>(center.y,center.x))<15){
                contourPoint1.x=contourPoint1.x+1;
                contourPoint1.y=contourPoint1.x*k+b;
                if(contourPoint1.y>=srcimg.rows||contourPoint1.x>=srcimg.cols||contourPoint1.y<0||contourPoint1.x<0)
                    break;
            }
            contours.push_back(cv::Point(contourPoint1.x,(contourPoint1.x)*k+b));
            while(!PointDense(srcimg,contourPoint2,radius,percent)||abs(srcimg.at<uchar>(contourPoint2.y,contourPoint2.x)-srcimg.at<uchar>(center.y,center.x))<15){
                contourPoint2.x=contourPoint2.x-1;
                contourPoint2.y=contourPoint2.x*k+b;
                if(contourPoint2.y>=srcimg.rows||contourPoint2.x>=srcimg.cols||contourPoint2.y<0||contourPoint2.x<0)
                    break;
            }
            contours.push_back(cv::Point(contourPoint2.x,(contourPoint2.x)*k+b));
        }
        else if(theta>=135&&theta<=180){
            cv::Point contourPoint1(center.x-1-radthresh,(center.x-1-radthresh)*k+b);
            cv::Point contourPoint2(center.x+1+radthresh,(center.x+1+radthresh)*k+b);
            while(!PointDense(srcimg,contourPoint1,radius,percent)||abs(srcimg.at<uchar>(contourPoint1.y,contourPoint1.x)-srcimg.at<uchar>(center.y,center.x))<15){
                contourPoint1.x=contourPoint1.x-1;
                contourPoint1.y=contourPoint1.x*k+b;
                if(contourPoint1.y>=srcimg.rows||contourPoint1.x>=srcimg.cols||contourPoint1.y<0||contourPoint1.x<0)
                    break;
            }
            contours.push_back(cv::Point(contourPoint1.x,(contourPoint1.x)*k+b));
            while(!PointDense(srcimg,contourPoint2,radius,percent)||abs(srcimg.at<uchar>(contourPoint2.y,contourPoint2.x)-srcimg.at<uchar>(center.y,center.x))<15){
                contourPoint2.x=contourPoint2.x+1;
                contourPoint2.y=contourPoint2.x*k+b;
                if(contourPoint2.y>=srcimg.rows||contourPoint2.x>=srcimg.cols||contourPoint2.y<0||contourPoint2.x<0)
                    break;
            }
            contours.push_back(cv::Point(contourPoint2.x,(contourPoint2.x)*k+b));
        }
        else{
            cv::Point contourPoint1((center.y+1+radthresh-b)/k,center.y+1+radthresh);
            cv::Point contourPoint2((center.y-1-radthresh-b)/k,center.y-1-radthresh);
            while(!PointDense(srcimg,contourPoint1,radius,percent)||abs(srcimg.at<uchar>(contourPoint1.y,contourPoint1.x)-srcimg.at<uchar>(center.y,center.x))<15){
                contourPoint1.y=contourPoint1.y+1;
                contourPoint1.x=(contourPoint1.y-b)/k;
                if(contourPoint1.y>=srcimg.rows||contourPoint1.x>=srcimg.cols||contourPoint1.y<0||contourPoint1.x<0)
                    break;
            }
            contours.push_back(cv::Point((contourPoint1.y-b)/k,contourPoint1.y));
            while(!PointDense(srcimg,contourPoint2,radius,percent)||abs(srcimg.at<uchar>(contourPoint2.y,contourPoint2.x)-srcimg.at<uchar>(center.y,center.x))<15){
                contourPoint2.y=contourPoint2.y-1;
                contourPoint2.x=(contourPoint2.y-b)/k;
                if(contourPoint2.y>=srcimg.rows||contourPoint2.x>=srcimg.cols||contourPoint2.y<0||contourPoint2.x<0)
                    break;
            }
            contours.push_back(cv::Point((contourPoint2.y-b)/k,contourPoint2.y));
        }
    }
    return 0;
}

//判断射线点周围的边缘密度，周围是以rad为半径的圆，超过thresh（0-1）判断为边缘，现在程序中写死rad=5,thresh=0.8
int MeasurE616RectExtract::PointDense(cv::Mat srcimg, cv::Point center, int rad, float thresh){
    if(srcimg.empty())
        return -1;
    if(srcimg.channels()==3)
        cv::cvtColor(srcimg,srcimg,CV_BGR2GRAY);
    cv::Mat roiimg;
    if(center.x-rad-1<0||center.y-rad-1<0||center.x+rad+1>=srcimg.cols||center.y+rad+1>=srcimg.rows)
    {return 0;}
    else{
        roiimg=srcimg(cv::Rect(cv::Point(center.x-rad-1,center.y-rad-1),cv::Point(center.x+rad+1,center.y+rad+1)));
    }
    cv::Mat maskimg=roiimg.clone();
    cv::circle(maskimg,cv::Point(maskimg.cols/2,maskimg.rows/2),rad,255,-1);
    int sum=0,num=0;
    for(int i=0;i<maskimg.rows;++i){
        for(int j=0;j<maskimg.cols;++j){
            if(srcimg.at<uchar>(center.y-rad-1+i,center.x-rad-1+j)==255&&maskimg.at<uchar>(i,j)==255){
                num++;
            }
            if(maskimg.at<uchar>(i,j)==255)
            {sum++;}
        }
    }
    float percent=(float)num/(float)sum;
    if(percent>=thresh){
        return 1;
    }
    else{return 0;}
}

//根据角度截取边缘点
vector<cv::Point> MeasurE616RectExtract::ContoursCut(vector<cv::Point> contours,int startangle,int endangle){
    vector<cv::Point> segcontours;
    if(startangle>endangle){
        for(int i=startangle;i<360;++i){
            if(i<180&&i>=1){segcontours.push_back(contours[359-(i-1)*2]);}
            else if(i>180&&i<=360){segcontours.push_back(contours[359-2*(i-180)+1]);}
            else if(i==180){segcontours.push_back(contours[1]);}
            else{segcontours.push_back(contours[0]);}
        }
        for(int i=0;i<endangle;++i){
            if(i<180&&i>=1){segcontours.push_back(contours[359-(i-1)*2]);}
            else if(i>180&&i<=360){segcontours.push_back(contours[359-2*(i-180)+1]);}
            else if(i==180){segcontours.push_back(contours[1]);}
            else{segcontours.push_back(contours[0]);}
        }
    }
    else{
        for(int i=startangle;i<endangle;++i){
            if(i<180&&i>=1){segcontours.push_back(contours[359-(i-1)*2]);}
            else if(i>180&&i<=360){segcontours.push_back(contours[359-2*(i-180)+1]);}
            else if(i==180){segcontours.push_back(contours[1]);}
            else{segcontours.push_back(contours[0]);}
        }
    }
    return segcontours;
}







//以下为提取后的操作，不属于边缘提取部分的代码
//拟合圆，返回圆心和半径
vector<float> MeasurE616RectExtract::FitCircle(vector<cv::Point>& contours){
    float a=0,b=0,r=0;
    double sumX=0.0,sumY=0.0,sumR=0.0;
    for(int i=0;i<contours.size();++i){
        sumX+=contours[i].x;
        sumY+=contours[i].y;
    }
    a=sumX/contours.size();
    b=sumY/contours.size();
    for(int i=0;i<contours.size();++i){
        sumR+=sqrt((contours[i].x-a)*(contours[i].x-a)+(contours[i].y-b)*(contours[i].y-b));
    }
    r=sumR/contours.size();
    return vector<float>{a,b,r};
}

//拟合圆，通过尝试的方法。除了边缘点集contours,还需要假设的圆心center和圆心尝试移动的范围threshold,返回circlepara为圆心和半径
void MeasurE616RectExtract::GetCircle(vector<cv::Point> contours, cv::Point center,int threshold,cv::Vec3f& circlepara){
    vector<double> sumvec;
    vector<cv::Point> pointvec;
    for(int i=center.x-threshold;i<center.x+threshold;++i){
        for(int j=center.y-threshold;j<center.y+threshold;++j){
            sumvec.push_back(CalcDiff(contours,cv::Point(i,j)));
            pointvec.push_back(cv::Point(i,j));
        }
    }
    vector<double>::iterator smallest = min_element(begin(sumvec), end(sumvec));
    int index=distance(std::begin(sumvec), smallest);
    circlepara[0]=pointvec[index].x;
    circlepara[1]=pointvec[index].y;
    circlepara[2]=sqrt((pointvec[index].x-contours[contours.size()/2].x)*(pointvec[index].x-contours[contours.size()/2].x)+(pointvec[index].y-contours[contours.size()/2].y)*(pointvec[index].y-contours[contours.size()/2].y));
}

//拟合圆GetCircle中计算差距，以每个点构成的半径和估计的半径之差作为评判标准
double MeasurE616RectExtract::CalcDiff(vector<cv::Point> contours,cv::Point center){
    float rad=(contours[contours.size()/2].x-center.x)*(contours[contours.size()/2].x-center.x)+(contours[contours.size()/2].y-center.y)*(contours[contours.size()/2].y-center.y);
    double sum=0;
    for(int i=0;i<contours.size();++i){
        float dist=fabs((contours[i].x-center.x)*(contours[i].x-center.x)+(contours[i].y-center.y)*(contours[i].y-center.y)-rad);
        sum+=dist;
    }
    return sum;
}

//计算圆上中点坐标，输入为圆的圆心和半径，切线斜率k和在圆上或者下 flag=0：上面 1：下面
cv::Point MeasurE616RectExtract::GetTanPoint(int a, int b, int R, float k, int flag){
    cv::Point tanpoint(0,0);
    if(flag==0){
        tanpoint.y=b-R/sqrt(1+k*k);
        tanpoint.x=a+k*R/sqrt(1+k*k);
    }
    else if(flag==1){
        tanpoint.y=b+R/sqrt(1+k*k);
        tanpoint.x=a-k*R/sqrt(1+k*k);
    }
    return tanpoint;
}

int MeasurE616RectExtract::KirschEdgeOuter(cv::Mat srcimg, int thresh, int num, float& dist){
    if(srcimg.empty())
        return -1;
    if(srcimg.channels()==3)
        cv::cvtColor(srcimg,srcimg,CV_BGR2GRAY);
    vector<cv::Point> seedpoints;
    vector<cv::Point> contours;
    int orientation;
    if(num==1){
        orientation=3;
        GetSeedPoints(srcimg,3,580,620,40,seedpoints);
        GetEdgePoint(srcimg, seedpoints,3,thresh,4,0.7,contours);
    }
    else if(num==2){
        orientation=3;
        GetSeedPoints(srcimg,3,800,900,40,seedpoints);
        GetEdgePoint(srcimg, seedpoints,3,thresh,4,0.6,contours);
    }
    else if(num==3){
        orientation=0;
        GetSrcEdgePoints(srcimg,0,450,500,90, contours);
        GetSrcEdgePoints(srcimg,0,1000,1050,90, contours);
        GetSrcEdgePoints(srcimg,0,1600,1650,90, contours);
    }
    else if(num==4){
        orientation=1;
        GetSeedPoints(srcimg,1,650,700,40,seedpoints);
        GetEdgePoint(srcimg, seedpoints,1,thresh,4,0.6,contours);
    }
    else if(num==5){
        orientation=0;
        GetSeedPointsSec(srcimg,2,420,440,40,seedpoints);
        GetSeedPointsSec(srcimg,2,685,715,40,seedpoints);
        GetEdgePoint(srcimg, seedpoints,0,thresh,4,0.6,contours);
    }
    else if(num==6){
        orientation=1;
        GetSeedPointsSec(srcimg,3,220,230,40,seedpoints);
        GetSeedPointsSec(srcimg,3,550,560,40,seedpoints);
        GetSeedPointsSec(srcimg,3,920,930,40,seedpoints);
        GetEdgePoint(srcimg, seedpoints,1,thresh,4,0.5,contours);
    }
    else if(num==7){
        orientation=2;
        GetSeedPointsSec(srcimg,0,240,260,40,seedpoints);
        GetSeedPointsSec(srcimg,0,820,825,40,seedpoints);
        GetEdgePoint(srcimg, seedpoints,2,thresh,4,0.6,contours);
    }
    else if(num==8){
        orientation=1;
        GetSeedPointsSec(srcimg,3,280,290,40,seedpoints);
        GetSeedPointsSec(srcimg,3,550,560,40,seedpoints);
        GetSeedPointsSec(srcimg,3,680,690,40,seedpoints);
        GetEdgePoint(srcimg, seedpoints,1,thresh,4,0.6,contours);
    }
    else if(num==9){
        orientation=2;
        GetSeedPointsSec(srcimg,0,810,820,40,seedpoints);
        GetSeedPointsSec(srcimg,0,1150,1160,40,seedpoints);
        GetEdgePoint(srcimg, seedpoints,2,thresh,4,0.6,contours);
    }
    float sum=0;
    for(int i=0;i<contours.size();++i){
        if(orientation==0||orientation==2){
            sum+=contours[i].x;
        }
        else{
            sum+=contours[i].y;
        }
    }
    dist=sum/contours.size();
    return 0;
}

void MeasurE616RectExtract::GetSeedPoints(cv::Mat srcimg,int orientation,int startline,int endline,int threshold, vector<cv::Point>& seedpoints){
    vector<vector<float>> pixelcurves;
    Pixel2Curve(srcimg, startline, endline, orientation, pixelcurves);
    vector<int> seedpointspos;
    GetSeedPos(pixelcurves,threshold,seedpointspos);
    for(int i=0;i<seedpointspos.size();++i){
        if(orientation==0){
            seedpoints.push_back(cv::Point(seedpointspos[i]+5,startline+i*2));
        }
        else if(orientation==1){
            seedpoints.push_back(cv::Point(startline+i*2,seedpointspos[i]+5));
        }
        else if(orientation==2){
            seedpoints.push_back(cv::Point(srcimg.cols-1-5-seedpointspos[i],startline+i*2));
        }
        else if(orientation==3){
            seedpoints.push_back(cv::Point(startline+i*2,srcimg.rows-1-5-seedpointspos[i]));
        }
    }
}

void MeasurE616RectExtract::GetEdgePoint(cv::Mat srcimg, vector<cv::Point> seedpoints, int orientation, int thresh,int radius,float percent,  vector<cv::Point>& contours){
    cv::Mat edge = krisch(srcimg);
    cv::threshold(edge, edge, thresh, 255, cv::THRESH_BINARY);
    if(orientation==3){
        for(int i=0;i<seedpoints.size();++i){
            cv::Point seed=seedpoints[i];
            cv::Point contourpoint(seed.x,seed.y-1);
            while(!PointDense(edge,contourpoint,radius,percent)||abs(edge.at<uchar>(contourpoint.y,contourpoint.x)-edge.at<uchar>(seed.y,seed.x))<15){
                contourpoint.y-=1;
                if(contourpoint.y==0)
                    break;
            }
            contours.push_back(contourpoint);
        }
    }
    else if(orientation==0){
        for(int i=0;i<seedpoints.size();++i){
            cv::Point seed=seedpoints[i];
            cv::Point contourpoint(seed.x+1,seed.y);
            while(!PointDense(edge,contourpoint,radius,percent)||abs(edge.at<uchar>(contourpoint.y,contourpoint.x)-edge.at<uchar>(seed.y,seed.x))<15){
                contourpoint.x+=1;
                if(contourpoint.x>=edge.cols)
                    break;
            }
            contours.push_back(contourpoint);
        }
    }
    else if(orientation==1){
        for(int i=0;i<seedpoints.size();++i){
            cv::Point seed=seedpoints[i];
            cv::Point contourpoint(seed.x,seed.y+1);
            while(!PointDense(edge,contourpoint,radius,percent)||abs(edge.at<uchar>(contourpoint.y,contourpoint.x)-edge.at<uchar>(seed.y,seed.x))<15){
                contourpoint.y+=1;
                if(contourpoint.y>=edge.rows)
                    break;
            }
            contours.push_back(contourpoint);
        }
    }
    else if(orientation==2){
        for(int i=0;i<seedpoints.size();++i){
            cv::Point seed=seedpoints[i];
            cv::Point contourpoint(seed.x-1,seed.y);
            while(!PointDense(edge,contourpoint,radius,percent)||abs(edge.at<uchar>(contourpoint.y,contourpoint.x)-edge.at<uchar>(seed.y,seed.x))<15){
                contourpoint.x-=1;
                if(contourpoint.x==0)
                    break;
            }
            contours.push_back(contourpoint);
        }
    }
}

int MeasurE616RectExtract::Pixel2Curve(cv::Mat srcimg,int startpos, int endpos, int orientation, vector<vector<float>>& curves){
    curves.clear();
    if(orientation==0){//从左往右
        for(int i=startpos;i<endpos-1;i=i+2){
            vector<float> curve;
            for(int j=5;j<srcimg.cols-1;++j){
                float kerneldata=(srcimg.at<uchar>(i,j)+srcimg.at<uchar>(i,j+1)+srcimg.at<uchar>(i+1,j)+srcimg.at<uchar>(i+1,j+1))/4;
                curve.push_back(kerneldata);
            }
            curves.push_back(curve);
        }
    }
    else if(orientation==1){//从上往下
        for(int i=startpos;i<endpos-1;i=i+2){
            vector<float> curve;
            for(int j=5;j<srcimg.rows-1;++j){
                float kerneldata=(srcimg.at<uchar>(j,i)+srcimg.at<uchar>(j,i+1)+srcimg.at<uchar>(j+1,i)+srcimg.at<uchar>(j+1,i+1))/4;
                curve.push_back(kerneldata);
            }
            curves.push_back(curve);
        }
    }
    else if(orientation==2){//从右往左
        for(int i=startpos;i<endpos-1;i=i+2){
            vector<float> curve;
            for(int j=srcimg.cols-1-5;j>0;--j){
                float kerneldata=(srcimg.at<uchar>(i,j)+srcimg.at<uchar>(i,j-1)+srcimg.at<uchar>(i+1,j)+srcimg.at<uchar>(i+1,j-1))/4;
                curve.push_back(kerneldata);
            }
            curves.push_back(curve);
        }
    }
    else if(orientation==3){//从下往上
        for(int i=startpos;i<endpos-1;i=i+2){
            vector<float> curve;
            for(int j=srcimg.rows-1-5;j>0;--j){
                float kerneldata=(srcimg.at<uchar>(j,i)+srcimg.at<uchar>(j,i+1)+srcimg.at<uchar>(j-1,i)+srcimg.at<uchar>(j-1,i+1))/4;
                curve.push_back(kerneldata);
            }
            curves.push_back(curve);
        }
    }
    return 0;
}

void MeasurE616RectExtract::GetSeedPos(vector<vector<float>> curves, int thresh, vector<int>& seedpoints){
    seedpoints.clear();
    for(int i=0;i<curves.size();++i){
        int startpos,endpos;
        int flag=0;
        for(int j=2;j<curves[i].size()-2;++j){
            if(curves[i][j-2]+curves[i][j-1]+curves[i][j]+curves[i][j+1]+curves[i][j+2]<thresh*5){
                if(!flag){
                    flag=1;
                    startpos=j;
                }
            }
            else{
                if(flag){
                    endpos=j;
                    break;
                }
            }
        }
        seedpoints.push_back((startpos+endpos)/2);
    }
}

void MeasurE616RectExtract::GetSrcEdgePoints(cv::Mat srcimg,int orientation,int startline,int endline,int threshold, vector<cv::Point>& edgepoints){
    vector<vector<float>> pixelcurves;
    Pixel2Curve(srcimg, startline, endline, orientation, pixelcurves);
    vector<int> seedpointspos;
    vector<int> edgepointspos;
    GetSeedPos(pixelcurves,40,seedpointspos);
    for(int i=0;i<pixelcurves.size();++i){
        int flag=0;
        for(int j=seedpointspos[i];j<pixelcurves[i].size();++j){
            if(pixelcurves[i][j]<threshold){
                flag=1;
            }
            else{
                if(flag){
                    edgepointspos.push_back(j);
                    break;
                }
                flag=0;
            }
        }
    }
    for(int i=0;i<edgepointspos.size();++i){
        if(orientation==0){
            edgepoints.push_back(cv::Point(edgepointspos[i]+5,startline+i*2));
        }
        else if(orientation==1){
            edgepoints.push_back(cv::Point(startline+i*2,edgepointspos[i]+5));
        }
        else if(orientation==2){
            edgepoints.push_back(cv::Point(srcimg.cols-1-5-edgepointspos[i],startline+i*2));
        }
        else if(orientation==3){
            edgepoints.push_back(cv::Point(startline+i*2,srcimg.rows-1-5-edgepointspos[i]));
        }
    }
}

void MeasurE616RectExtract::GetSeedPointsSec(cv::Mat srcimg,int orientation,int startline,int endline,int threshold, vector<cv::Point>& seedpoints){
    vector<vector<float>> pixelcurves;
    Pixel2Curve(srcimg, startline, endline, orientation, pixelcurves);
    vector<int> seedpointspos;
    GetSeedPosSecond(pixelcurves,threshold,seedpointspos);
    for(int i=0;i<seedpointspos.size();++i){
        if(orientation==0){
            seedpoints.push_back(cv::Point(seedpointspos[i]+5,startline+i*2));
        }
        else if(orientation==1){
            seedpoints.push_back(cv::Point(startline+i*2,seedpointspos[i]+5));
        }
        else if(orientation==2){
            seedpoints.push_back(cv::Point(srcimg.cols-1-5-seedpointspos[i],startline+i*2));
        }
        else if(orientation==3){
            seedpoints.push_back(cv::Point(startline+i*2,srcimg.rows-1-5-seedpointspos[i]));
        }
    }
}

void MeasurE616RectExtract::GetSeedPosSecond(vector<vector<float>> curves,int thresh, vector<int>& seedpoints){
    seedpoints.clear();
    for(int i=0;i<curves.size();++i){
        int startpos,endpos;
        int flag=0,num=0;
        for(int j=2;j<curves[i].size()-2;++j){
            if(curves[i][j-2]+curves[i][j-1]+curves[i][j]+curves[i][j+1]+curves[i][j+2]<thresh*5){
                flag=1;
                if(flag&&num==1){
                    startpos=j;
                    num++;
                }
            }
            else{
                if(flag){
                    num++;
                }
                if(num==3){
                    endpos=j;
                    break;
                }
                flag=0;
            }
        }
        seedpoints.push_back((startpos+endpos)/2);
    }
}

float MeasurE616RectExtract::GetMidLineY(vector<vector<cv::Point>> linecontours, int pos){
    vector<float> toplinepara,bottomlinepara;
    LineFitLeastSquares(linecontours[0],toplinepara);
    LineFitLeastSquares(linecontours[1],bottomlinepara);
    float ypos=(toplinepara[0]*pos+toplinepara[1]+bottomlinepara[0]*pos+bottomlinepara[1])/2.0;
    return ypos;
}

float MeasurE616RectExtract::GetLineX(vector<cv::Point> linecontour, int pos){
    vector<float> leftlinepara;
    for(int i=0;i<linecontour.size();++i){
        cv::Point temp=linecontour[i];
        linecontour[i].x=temp.y;
        linecontour[i].y=temp.x;
    }
    LineFitLeastSquares(linecontour,leftlinepara);
    float xpos=leftlinepara[0]*pos+leftlinepara[1];
    return xpos;
}
